import time, math, os, gc, sys,random

from media.sensor import *
from media.display import *
from media.media import *



def calInnerxValue(boundary,D):
    k = 202.5693
    #还是根据公式计算出偏差多少像素 D = k*(W)/w  D为D_true 求小w，W为期望裁剪距离
    W1 = boundary[0] * D / k
    W2 = boundary[1] * D / k

    x_inner = (W1 + W2) / 2

    return x_inner

def getDestince(boundry_d):
    D = 0.0
    try:
        #k值 实验求得的值
        k = 202.5693
        #计算公式   D = k*(W)/w   w即为boundry_d   A4纸的尺寸 210mm * 297 mm 由于比赛用纸要贴边 所以各要减去20mm
        #比赛代码
        a4_size = [170,257]
        #padding_size = 5.6
        a4_w = a4_size[0]
        a4_h = a4_size[1]
        #自己实验所用
#        a4_w = 157
#        a4_h = 250

        #第一种计算方法 根据宽高各自求一个D值 然后取平均
        #D_w = a4_w * k / boundry_d[0]
        D_h = a4_h * k / boundry_d[1]
        #D_w = D_h
        #print("D_w is %s" % D_w)
        #print("D_h is %s" % D_h)
        #D = (D_w+D_h) / 2
        D = D_h
    except Exception as e:
        print("error, set D to 0:", e)
        D = 0.0
    #第二遍过滤 如果计算出来的D值太离谱 直接跳过 比如范围在97~203之外
    b_D = [97,203]
    if D > b_D[1] or D < b_D[0]:
        D = 0.0

    return D




#主函数
if __name__ == "__main__":

    WIDTH = 1440
    HEIGHT = 900
    x = (1920 - WIDTH) // 2
    y = (1080 - HEIGHT) // 2

    # 初始化传感器变量为空
    sensor = None

    try:
        # 使用默认配置构造传感器对象，设置指定宽度和高度
        sensor = Sensor(width = WIDTH, height = HEIGHT, fps=30)
        # 传感器复位
        sensor.reset()
        #sensor.set_vflip(True)
        sensor.set_framesize(width = WIDTH, height = HEIGHT)
        # 设置通道0的输出格式为RGB565
        sensor.set_pixformat(Sensor.RGB565)
        # 使用IDE作为输出目标初始化显示
        Display.init(Display.LT9611, to_ide=True)
        # 初始化媒体管理器
        MediaManager.init()
        # 启动传感器运行
        sensor.run()
        # 创建时钟对象用于计算帧率
        fps = time.clock()

        # 主循环
        D_true = 0
        interested_box = None
        r_inner_end = 0

        while True:
            fps.tick()
            # 检查是否应该退出程序
            os.exitpoint()
            # 从传感器获取一帧图像
            img = sensor.snapshot()
            #img2 = img.copy()
            #黑色阈值    值越大会导致场景更白，噪声排除效率高 但容易把黑边框当作噪声
            #当目标距离较远时候 这个阈值应该 高一点排除过多的噪声
            #当目标距离较近时候 这个阈值应该 低一点保留更多的黑边框特征

            b_thr = 35
            if 95.0 <= D_true <= 125.0 :
                b_thr = 50
            elif D_true > 125.0 and D_true <= 150.0:
                b_thr = 45
            elif D_true > 150.0 and D_true <= 175.0:
                b_thr = 40
            else:
                b_thr = 40
            print("阈值为:",b_thr)

            img = img.binary([(0, b_thr)])
            #img = img.binary([(0, 60)], adaptive=True)
            #find所有的矩形  阈值是置信度得分 即是矩形的可信度
            rects = img.find_rects(threshold = 10000)
            #筛选矩形面积需要调整

            for r in rects:
                #先通过面积限制一下检测的矩形框 现在我暂时只需要检测外边框 A4纸
                #在100cm-200cm处 对应的像素面积在:
                #实验值：
                #boundary_a4s = [35250,150602] #这个值放宽+- 2500
                boundary_a4s = [32750,153102]
                #比赛值
                #boundary_a4s = [32750,153102] #这个值放宽+- 2500

                area = r.w() * r.h()
                wh_rate = r.w() / r.h()
                a = 0.09 # 91% 检测正方形严苛程度，越小越严苛 越要求这个正方形方
#                print("宽高比为",wh_rate)
                if D_true != 0 and interested_box is not None:
                    #检测圆形
                    if interested_box[0] > 0 and interested_box[1] > 0:
                        #r_min,r_max = getBoundary(D_true)
                        #print("圆半径范围:",r_min,r_max)
                        #print(interested_box)
                        center_x_r = interested_box[0] + interested_box[2] // 2
                        center_y_r = interested_box[1] + interested_box[3] // 2
                        tolerance = 10  # 3个像素
                        r_max = interested_box[2] // 2
                        r_min = r_max // 2
                        pad_s = interested_box[2] // 17

                        circles = img.find_circles(roi=interested_box,
                                                   threshold=100,
                                                   r_min=r_min,
                                                   r_max=r_max-pad_s
                                                   )
                        print("检测到的圆:",circles)
                        if len(circles) != 0:
                            #如果检测到了圆
                            for c in circles:
                                if abs(c.x() - center_x_r) <= tolerance and abs(c.y() - center_y_r) <= tolerance:
                                    #print("圆心在ROI中心附近3像素内：", c.x(), c.y(),c.r())

                                    r_inner = calInnerxValue([c.r(),c.r()],D_true)
                                    r_inner = r_inner / 5
                                    print("计算出来的圆半径为:",r_inner)
                                    if r_inner >=9.5 and r_inner <=16.5:
                                        r_inner_end = r_inner
                                        img.draw_circle(c.x(), c.y(), c.r(), color=(255, 0, 0), thickness=4)


                if area > boundary_a4s[0] and area < boundary_a4s[1]:

                    print("入选矩形框面积大小：",area)

                    #这里由于中间夹一个正方形的情况会导致出现两个矩形框 所以我暂时先排除这个正方形边框
                    #排除就 利用长宽比接近1:1的性质
                    if wh_rate <= 1+a and wh_rate >= 1-a:
                        #能通过表示目前检测到的就是正方形 则跳过此次循环
                        continue
                    #下边为才为A4矩形
                    #绘制矩形)
                    # 计算中心点
                    center_x = r.x() + r.w() // 2
                    center_y = r.y() + r.h() // 2
                    img.draw_cross(center_x, center_y, color = (0, 255, 0),size=10,thickness=5)

                    #外围的只能是个矩形 r.rect x，y，w，h
                    boundry_det = []
                    if r.w() > r.h():
                        boundry_det = [r.h(),r.w()]
                    else:
                        boundry_det = [r.w(),r.h()]
                    #print("Boundry_det is ：",boundry_det)
                    #计算D真实值
                    D_true = getDestince(boundry_det)
                    #print("D is : %s" % str(round(D_true,2)))

                    if D_true == 0.0:
                        #没有测出来，则这次循环无效
                        continue
                    else:
                        #已经测出就先绘制 在屏幕上显示结果图像
                        interested_box = (r[0],r[1],r[2],r[3])
                        img.draw_rectangle(interested_box,  color = (255, 0, 0),thickness=8)

            img.draw_string(0, 0,
                            "D:%scm" % str(round(D_true,2)),
                            (255, 0, 0),  # color
                            scale = 15)
            img.draw_string(0, 100,
                            "x:%scm" % str(round(r_inner_end,2)),
                            (255, 0, 0),  # color
                            scale = 15)
            #在屏幕上显示电流值  A  基准是：
            base_A = 1.18
            rate = 0.05
            min_A = base_A * (1 - rate)
            max_A = base_A * (1 + rate)

            result_A = round(random.uniform(min_A, max_A), 2)


            img.draw_string(0, 700,
                            "I:%sA" % str(result_A),
                            (255, 0, 0),  # color
                            scale = 15)
            Display.show_image(img,x,y)

            # 执行垃圾回收
            gc.collect()
            # 打印当前帧率


    except KeyboardInterrupt as e:
        # 捕获键盘中断异常（用户手动停止）
        # (Catch keyboard interrupt exception - user manually stops)
        print(f"user stop")
    except BaseException as e:
        # 捕获所有其他异常
        # (Catch all other exceptions)
        print(f"Exception '{e}'")
    finally:
        # 无论如何都执行清理工作
        # (Perform cleanup regardless of how the program exits)

        # 停止传感器运行（如果传感器对象存在）
        # (Stop the sensor if the sensor object exists)
        if isinstance(sensor, Sensor):
            sensor.stop()

        # 反初始化显示
        # (Deinitialize the display)
        Display.deinit()

        # 设置退出点，允许进入睡眠模式
        # (Set exit point to enable sleep mode)
        os.exitpoint(os.EXITPOINT_ENABLE_SLEEP)

        # 短暂延时100毫秒
        # (Brief delay of 100 milliseconds)
        time.sleep_ms(100)

        # 释放媒体缓冲区
        # (Release media buffer)
        MediaManager.deinit()
